/* ITU-T G.729 Software Package Release 2 (November 2006) */
/*
   ITU-T G.729 Annex C - Reference C code for floating point
                         implementation of G.729
                         Version 1.01 of 15.September.98
*/

/*
----------------------------------------------------------------------
                    COPYRIGHT NOTICE
----------------------------------------------------------------------
   ITU-T G.729 Annex C ANSI C source code
   Copyright (C) 1998, AT&T, France Telecom, NTT, University of
   Sherbrooke.  All rights reserved.

----------------------------------------------------------------------
*/

/*
 File : PITCH.C
 Used for the floating point version of G.729 main body
 (not for G.729A)
*/

/*****************************************************************************/
/*           Long Term Prediction Routines                                   */
/*****************************************************************************/
#include <math.h>
#include "typedef.h"
#include "ld8k.h"
#include "tab_ld8k.h"

/* prototypes for local functions */
static void  norm_corr(FLOAT exc[], FLOAT xn[], FLOAT h[], int l_subfr,
    int t_min, int t_max, FLOAT corr_norm[]);
static FLOAT interpol_3(FLOAT cor[], int frac);
static int   lag_max(FLOAT signal[],int L_frame,int pit_max,
                  int pit_min, FLOAT *cor_max);
static FLOAT inv_sqrt( FLOAT x  );

/*----------------------------------------------------------------------------
 * pitch_ol -  compute the open loop pitch lag
 *----------------------------------------------------------------------------
 */
int pitch_ol(           /* output: open-loop pitch lag */
 FLOAT signal[],        /* input : signal to compute pitch  */
                        /*         s[-PIT_MAX : l_frame-1]  */
   int pit_min,         /* input : minimum pitch lag                          */
   int pit_max,         /* input : maximum pitch lag                          */
   int l_frame          /* input : error minimization window */
)
{
    FLOAT  max1, max2, max3;
    int    p_max1, p_max2, p_max3;

   /*--------------------------------------------------------------------*
    *  The pitch lag search is divided in three sections.                *
    *  Each section cannot have a pitch multiple.                        *
    *  We find a maximum for each section.                               *
    *  We compare the maxima of each section by favoring small lag.      *
    *                                                                    *
    *  First section:  lag delay = PIT_MAX to 80                         *
    *  Second section: lag delay = 79 to 40                              *
    *  Third section:  lag delay = 39 to 20                              *
    *--------------------------------------------------------------------*/

    p_max1 = lag_max(signal, l_frame, pit_max, 80 , &max1);
    p_max2 = lag_max(signal, l_frame, 79     , 40 , &max2);
    p_max3 = lag_max(signal, l_frame, 39     , pit_min , &max3);

   /*--------------------------------------------------------------------*
    * Compare the 3 sections maxima, and favor small lag.                *
    *--------------------------------------------------------------------*/

    if ( max1 * THRESHPIT < max2 ) {
        max1 = max2;
        p_max1 = p_max2;
    }

    if ( max1 * THRESHPIT < max3 )  p_max1 = p_max3;

    return (p_max1);
}
/*----------------------------------------------------------------------------
 * lag_max - Find the lag that has maximum correlation
 *----------------------------------------------------------------------------
 */
static int lag_max(     /* output: lag found */
  FLOAT signal[],       /* input : Signal to compute the open loop pitch
                                   signal[-142:-1] should be known.       */
  int l_frame,          /* input : Length of frame to compute pitch       */
  int lagmax,           /* input : maximum lag                            */
  int lagmin,           /* input : minimum lag                            */
  FLOAT *cor_max        /* input : normalized correlation of selected lag */
)
{
    int    i, j;
    FLOAT  *p, *p1;
    FLOAT  max, t0;
    int    p_max;

    max = FLT_MIN_G729;

    for (i = lagmax; i >= lagmin; i--) {
        p  = signal;
        p1 = &signal[-i];
        t0 = (F)0.0;

        for (j=0; j<l_frame; j++) {
            t0 += *p++ * *p1++;
        }

        if (t0 >= max) {
            max    = t0;
            p_max = i;
        }
    }

    /* compute energy */

    t0 = (F)0.01;                  /* to avoid division by zero */
    p = &signal[-p_max];
    for(i=0; i<l_frame; i++, p++) {
        t0 += *p * *p;
    }
    t0 = inv_sqrt(t0);          /* 1/sqrt(energy)    */

    *cor_max = max * t0;        /* max/sqrt(energy)  */

    return(p_max);
}


/*----------------------------------------------------------------------------
 * pitch_fr3 - find the pitch period  with 1/3 subsample resolution
 *----------------------------------------------------------------------------
 */
int pitch_fr3(          /* output: integer part of pitch period        */
 FLOAT exc[],           /* input : excitation buffer                   */
 FLOAT xn[],            /* input : target vector                       */
 FLOAT h[],             /* input : impulse response of filters.        */
 int l_subfr,           /* input : Length of frame to compute pitch    */
 int t0_min,            /* input : minimum value in the searched range */
 int t0_max,            /* input : maximum value in the searched range */
 int i_subfr,           /* input : indicator for first subframe        */
 int *pit_frac          /* output: chosen fraction                     */
)
{
  int    i, frac;
  int    lag, t_min, t_max;
  FLOAT  max;
  FLOAT  corr_int;
  FLOAT  corr_v[10+2*L_INTER4];  /* size: 2*L_INTER4+t0_max-t0_min+1 */
  FLOAT  *corr;

  /* Find interval to compute normalized correlation */

  t_min = t0_min - L_INTER4;
  t_max = t0_max + L_INTER4;

  corr = &corr_v[-t_min];    /* corr[t_min:t_max] */

  /* Compute normalized correlation between target and filtered excitation */

  norm_corr(exc, xn, h, l_subfr, t_min, t_max, corr);

  /* find integer pitch */

  max = corr[t0_min];
  lag  = t0_min;

  for(i= t0_min+1; i<=t0_max; i++)
  {
    if( corr[i] >= max)
    {
      max = corr[i];
      lag = i;
    }
  }

  /* If first subframe and lag > 84 do not search fractionnal pitch */

  if( (i_subfr == 0) && (lag > 84) )
  {
    *pit_frac = 0;
    return(lag);
  }

  /* test the fractions around lag and choose the one which maximizes
     the interpolated normalized correlation */

  max  = interpol_3(&corr[lag], -2);
  frac = -2;

  for (i = -1; i <= 2; i++)
  {
    corr_int = interpol_3(&corr[lag], i);
    if(corr_int > max)
    {
      max = corr_int;
      frac = i;
    }
  }

  /* limit the fraction value in the interval [-1,0,1] */

  if (frac == -2)
  {
    frac = 1;
    lag -= 1;
  }
  if (frac == 2)
  {
    frac = -1;
    lag += 1;
  }

  *pit_frac = frac;

  return lag;
}

/*----------------------------------------------------------------------------
 * norm_corr - Find the normalized correlation between the target vector and
 *             the filtered past excitation.
 *----------------------------------------------------------------------------
 */
static void norm_corr(
 FLOAT exc[],           /* input : excitation buffer */
 FLOAT xn[],            /* input : target vector */
 FLOAT h[],             /* input : imp response of synth and weighting flt */
 int l_subfr,           /* input : Length of frame to compute pitch */
 int t_min,             /* input : minimum value of searched range */
 int t_max,             /* input : maximum value of search range */
 FLOAT corr_norm[]      /* output: normalized correlation (correlation between
                                   target and filtered excitation divided by
                                   the square root of energy of filtered
                                    excitation) */
)
{
 int    i, j, k;
 FLOAT excf[L_SUBFR];     /* filtered past excitation */
 FLOAT  alp, s, norm;

 k = -t_min;

 /* compute the filtered excitation for the first delay t_min */

 convolve(&exc[k], h, excf, l_subfr);

 /* loop for every possible period */

 for (i = t_min; i <= t_max; i++)
 {
   /* Compute 1/sqrt(energie of excf[]) */

   alp = (F)0.01;
   for (j = 0; j < l_subfr; j++)
     alp += excf[j]*excf[j];

   norm = inv_sqrt(alp);


   /* Compute correlation between xn[] and excf[] */

   s = (F)0.0;
   for (j = 0; j < l_subfr; j++)  s += xn[j]*excf[j];


   /* Normalize correlation = correlation * (1/sqrt(energie)) */

   corr_norm[i] = s*norm;

   /* modify the filtered excitation excf[] for the next iteration */

   if (i != t_max)
   {
     k--;
     for (j = l_subfr-1; j > 0; j--)
        excf[j] = excf[j-1] + exc[k]*h[j];
     excf[0] = exc[k];
   }
 }

 return;

}
/*----------------------------------------------------------------------------
 * g_pitch - compute adaptive codebook gain and compute <y1,y1> , -2<xn,y1>
 *----------------------------------------------------------------------------
 */

FLOAT g_pitch(          /* output: pitch gain */
 FLOAT xn[],            /* input : target vector */
 FLOAT y1[],            /* input : filtered adaptive codebook vector */
 FLOAT g_coeff[],       /* output: <y1,y1> and -2<xn,y1> */
 int l_subfr            /* input : vector dimension */
)
{
    FLOAT xy, yy, gain;
    int   i;

    xy = (F)0.0;
    for (i = 0; i < l_subfr; i++) {
        xy += xn[i] * y1[i];
    }
    yy = (F)0.01;
    for (i = 0; i < l_subfr; i++) {
        yy += y1[i] * y1[i];          /* energy of filtered excitation */
    }
    g_coeff[0] = yy;
    g_coeff[1] = (F)-2.0*xy +(F)0.01;

    /* find pitch gain and bound it by [0,1.2] */

    gain = xy/yy;

    if (gain<(F)0.0)  gain = (F)0.0;
    if (gain>GAIN_PIT_MAX) gain = GAIN_PIT_MAX;

    return gain;
}

/*----------------------------------------------------------------------*
 *    Function enc_lag3()                                               *
 *             ~~~~~~~~~~                                               *
 *   Encoding of fractional pitch lag with 1/3 resolution.              *
 *----------------------------------------------------------------------*
 * The pitch range for the first subframe is divided as follows:        *
 *   19 1/3  to   84 2/3   resolution 1/3                               *
 *   85      to   143      resolution 1                                 *
 *                                                                      *
 * The period in the first subframe is encoded with 8 bits.             *
 * For the range with fractions:                                        *
 *   index = (T-19)*3 + frac - 1;   where T=[19..85] and frac=[-1,0,1]  *
 * and for the integer only range                                       *
 *   index = (T - 85) + 197;        where T=[86..143]                   *
 *----------------------------------------------------------------------*
 * For the second subframe a resolution of 1/3 is always used, and the  *
 * search range is relative to the lag in the first subframe.           *
 * If t0 is the lag in the first subframe then                          *
 *  t_min=t0-5   and  t_max=t0+4   and  the range is given by           *
 *       t_min - 2/3   to  t_max + 2/3                                  *
 *                                                                      *
 * The period in the 2nd subframe is encoded with 5 bits:               *
 *   index = (T-(t_min-1))*3 + frac - 1;    where T[t_min-1 .. t_max+1] *
 *----------------------------------------------------------------------*/
int  enc_lag3(     /* output: Return index of encoding */
  int  T0,         /* input : Pitch delay              */
  int  T0_frac,    /* input : Fractional pitch delay   */
  int  *T0_min,    /* in/out: Minimum search delay     */
  int  *T0_max,    /* in/out: Maximum search delay     */
  int pit_min,     /* input : Minimum pitch delay      */
  int pit_max,     /* input : Maximum pitch delay      */
  int  pit_flag    /* input : Flag for 1st subframe    */
)
{
  int index;

  if (pit_flag == 0)   /* if 1st subframe */
  {
     /* encode pitch delay (with fraction) */

     if (T0 <= 85)
       index = T0*3 - 58 + T0_frac;
     else
       index = T0 + 112;

     /* find T0_min and T0_max for second subframe */

     *T0_min = T0 - 5;
     if (*T0_min < pit_min) *T0_min = pit_min;
     *T0_max = *T0_min + 9;
     if (*T0_max > pit_max)
     {
         *T0_max = pit_max;
         *T0_min = *T0_max - 9;
     }
  }

  else                    /* second subframe */
  {
     index = T0 - *T0_min;
     index = index*3 + 2 + T0_frac;
  }
  return index;
}

/*----------------------------------------------------------------------------
 * interpol_3 - For interpolating the normalized correlation
 *----------------------------------------------------------------------------
 */
static FLOAT interpol_3(   /* output: interpolated value */
 FLOAT *x,              /* input : function to be interpolated */
 int frac               /* input : fraction value to evaluate */
)
{
  int i;
  FLOAT s, *x1, *x2, *c1, *c2;


  if (frac < 0) {
    frac += UP_SAMP;
    x--;
  }
  x1 = &x[0];
  x2 = &x[1];
  c1 = &inter_3[frac];
  c2 = &inter_3[UP_SAMP-frac];

  s = (F)0.0;
  for(i=0; i< L_INTER4; i++, c1+=UP_SAMP, c2+=UP_SAMP)
     s+= (*x1--) * (*c1) + (*x2++) * (*c2);

  return s;
}

/*----------------------------------------------------------------------------
 * inv_sqrt - compute y = 1 / sqrt(x)
 *----------------------------------------------------------------------------
 */
static FLOAT inv_sqrt(         /* output: 1/sqrt(x) */
 FLOAT x                /* input : value of x */
)
{
   return ((F)1.0 / (FLOAT)sqrt((double)x) );
}
